#include "matrixcom.h"

namespace CoreBase
{
	void MatrixCom::transpose(osg::Matrix& dest, const osg::Matrix& src)
	{
		for (int i = 0; i < 4; ++i)
		{
			for (int j = 0; j < 4; ++j)
			{
				dest(i,j) = src(j,i);
			}
		}
	}

	osg::Vec3 MatrixCom::getRow3(const osg::Matrix& matrix, int row)
	{
		return osg::Vec3(matrix(row,0), matrix(row, 1), matrix(row,2));
	}

	osg::Vec4 MatrixCom::getRow4(const osg::Matrix& matrix, int row)
	{
		return osg::Vec4(matrix(row,0), matrix(row,1), matrix(row,2), matrix(row,3));
	}

	void MatrixCom::setRow(osg::Matrix& matrix, const osg::Vec3& vec, int row)
	{
		for (int i = 0; i < 3; ++i)
		{
			matrix(row,i) = vec[i];
		}
	}

	void MatrixCom::setRow(osg::Matrix& matrix, const osg::Vec4& vec, int row)
	{
		for (int i = 0; i < 4; ++i)
		{
			matrix(row, i) = vec[i];
		}
	}

	void MatrixCom::hprToMatrix(osg::Matrix& rotation, const osg::Vec3& hpr)
	{
		double ch, sh, cp, sp, cr, sr, srsp, crsp, srcp;
		const osg::Vec3::value_type magic_epsilon = (osg::Vec3::value_type)0.00001;
		if (osg::equivalent(hpr[0], (osg::Vec3::value_type)0.0, magic_epsilon))
		{
			ch = 1.0;
			sh = 0.0;
		}
		else
		{
			sh = sinf(osg::DegreesToRadians(hpr[0]));
			ch = cosf(osg::DegreesToRadians(hpr[0]));
		}
		if (osg::equivalent(hpr[1], (osg::Vec3::value_type)0.0, magic_epsilon))
		{
			cp = 1.0;
			sp = 0.0;
		}
		else
		{
			sp = sinf(osg::DegreesToRadians(hpr[1]));
			cp = cosf(osg::DegreesToRadians(hpr[1]));
		}
		if (osg::equivalent(hpr[2], (osg::Vec3::value_type)0.0, magic_epsilon))
		{
			cr   = 1.0;
			sr   = 0.0;
			srsp = 0.0;
			srcp = 0.0;
			crsp = sp;
		}
		else
		{
			sr   = sinf(osg::DegreesToRadians(hpr[2]));
			cr   = cosf(osg::DegreesToRadians(hpr[2]));
			srsp = sr * sp;
			crsp = cr * sp;
			srcp = sr * cp;
		}
		rotation(0, 0) =  ch * cr - sh * srsp;
		rotation(1, 0) = -sh * cp;
		rotation(2, 0) =  sr * ch + sh * crsp;
		rotation(0, 1) =  cr * sh + srsp * ch;
		rotation(1, 1) =  ch * cp;
		rotation(2, 1) =  sr * sh - crsp * ch;
		rotation(0, 2) = -srcp;
		rotation(1, 2) =  sp;
		rotation(2, 2) =  cr * cp;
		rotation(3, 0) =  0.0;
		rotation(3, 1) =  0.0;
		rotation(3, 2) =  0.0;
		rotation(0, 3) =  0.0;
		rotation(1, 3) =  0.0;
		rotation(2, 3) =  0.0;
		rotation(3, 3) =  1.0;
	}

	void MatrixCom::positionAndHprToMatrix(osg::Matrix& rotation, const osg::Vec3& xyz, const osg::Vec3& hpr)
	{
		hprToMatrix(rotation, hpr);
		rotation(3, 0) = xyz[0];
		rotation(3, 1) = xyz[1];
		rotation(3, 2) = xyz[2];
	}

	void MatrixCom::matrixToHpr(osg::Vec3& hpr, const osg::Matrix& rotation)
	{
		osg::Matrix mat;
		osg::Vec3 col1(rotation(0, 0), rotation(0, 1), rotation(0, 2));
		double s = col1.length();
		const double magic_epsilon = 0.00001;
		if (s <= magic_epsilon)
		{
			hpr.set(0.0f, 0.0f, 0.0f);
			return;
		}
		double oneOverS = 1.0f / s;
		for (int i = 0; i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				mat(i, j) = rotation(i, j) * oneOverS;
			}
		}
		double sin_pitch = clampUnity(mat(1, 2));
		double pitch = asin(sin_pitch);
		hpr[1] = osg::RadiansToDegrees(pitch);
		double cp = cos(pitch);
		if (cp > -magic_epsilon && cp < magic_epsilon)
		{
			double cr = clampUnity(-mat(2,1));
			double sr = clampUnity(mat(0,1));
			hpr[0] = 0.0f;
			hpr[2] = osg::RadiansToDegrees(atan2(sr,cr));
		}
		else
		{
			double one_over_cp = 1.0 / cp;
			double sr = clampUnity(-mat(0,2) * one_over_cp);
			double cr = clampUnity( mat(2,2) * one_over_cp);
			double sh = clampUnity(-mat(1,0) * one_over_cp);
			double ch = clampUnity( mat(1,1) * one_over_cp);
			if ((osg::equivalent(sh,0.0,magic_epsilon) && osg::equivalent(ch,0.0,magic_epsilon)) ||
				(osg::equivalent(sr,0.0,magic_epsilon) && osg::equivalent(cr,0.0,magic_epsilon)) )
			{
				cr = clampUnity(-mat(2,1));
				sr = clampUnity(mat(0,1));;
				hpr[0] = 0.0f;
			}
			else
			{
				hpr[0] = osg::RadiansToDegrees(atan2(sh, ch));
			}
			hpr[2] = osg::RadiansToDegrees(atan2(sr, cr));
		}
	}

	float MatrixCom::clampUnity(float x)
	{
		if (x >  1.0f) { return  1.0f; }
		if (x < -1.0f) { return -1.0f; }
		return x;
	}

	void MatrixCom::matrixToHprAndPosition(osg::Vec3& xyz, osg::Vec3& hpr, const osg::Matrix& rotation)
	{
		matrixToHpr(hpr, rotation);
		xyz[0] = rotation(3, 0);
		xyz[1] = rotation(3, 1);
		xyz[2] = rotation(3, 2);
	}

	void MatrixCom::transformVec3(osg::Vec3& xyz, const osg::Matrix& transformMat)
	{
		transformVec3(xyz, xyz, transformMat);
	}

	void MatrixCom::transformVec3(osg::Vec3& vec_in, const osg::Vec3& xyz, const osg::Matrix& transformMat)
	{
		vec_in = osg::Matrix::transform3x3(xyz, transformMat);
		vec_in[0] += transformMat(3,0);
		vec_in[1] += transformMat(3,1);
		vec_in[2] += transformMat(3,2);
	}

	void MatrixCom::matCopyMinusEqual(osg::Matrix& C, const osg::Matrix& A)
	{
		for (int i = 0; i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				C(i,j) -= A(i,j);
			}
		}
	}

	void MatrixCom::matBinOpEqualPlus(osg::Matrix& C, const float g1, const osg::Matrix& A, const float g2, const osg::Matrix& B)
	{
		for (int i = 0; i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				C(i, j) = g1 * A(i, j) + g2 * B(i, j);
			}
		}
	}

	void MatrixCom::adjointTranspose(const osg::Matrix& M, osg::Matrix& MadjT)
	{
		MatrixCom::setRow(MadjT, MatrixCom::getRow3(M, 1)^MatrixCom::getRow3(M, 2), 0);
		MatrixCom::setRow(MadjT, MatrixCom::getRow3(M, 2)^MatrixCom::getRow3(M, 0), 1);
		MatrixCom::setRow(MadjT, MatrixCom::getRow3(M, 0)^MatrixCom::getRow3(M, 1), 2);
	}

	float MatrixCom::normInf(const osg::Matrix& M)
	{
		return matNorm(M, 0);
	}

	float MatrixCom::normOne(const osg::Matrix& M)
	{
		return matNorm(M, 1);
	}

	int MatrixCom::findMaxCol(const osg::Matrix& M)
	{
		float abs;
		float max = 0.0;
		int col = -1;
		for (int i = 0; i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				abs = M(i,j);

				if (abs < 0.0)
				{
					abs = -abs;
				}
				if (abs > max)
				{
					max = abs;
					col = j;
				}
			}
		}
		return col;
	}

	void MatrixCom::makeReflector(const osg::Vec3& v, osg::Vec3& u)
	{
		float s = sqrt(v * v);
		u[0] = v[0];
		u[1] = v[1];
		u[2] = v[2] + ((v[2] < 0.0) ? -s : s);
		s = sqrt(2.0 / (u * u));
		u[0] = u[0] * s;
		u[1] = u[1] * s;
		u[2] = u[2] * s;
	}

	void MatrixCom::reflectCols(osg::Matrix& M, const osg::Vec3& u)
	{
		for (int i = 0; i < 3; ++i)
		{
			float s = u[0] * M(0,i) + u[1] * M(1,i) + u[2] * M(2,i);
			for (int j = 0; j < 3; ++j)
			{
				M(j,i) -= u[j] * s;
			}
		}
	}

	void MatrixCom::reflectRows(osg::Matrix& M, const osg::Vec3& u)
	{
		for (int i = 0; i < 3; ++i)
		{
			float s = u * MatrixCom::getRow3(M,i);
			for (int j = 0; j < 3; ++j)
			{
				M(i,j) -= u[j] * s;
			}
		}
	}

	float MatrixCom::matNorm(const osg::Matrix& M, const int tpose)
	{
		float sum;
		float max = 0.0;
		for (int i = 0; i < 3; ++i)
		{
			if (tpose)
			{
				sum = std::abs(M(0, i)) + std::abs(M(1, i)) + std::abs(M(2, i));
			}
			else
			{
				sum = std::abs(M(i, 0))+std::abs(M(i, 1))+std::abs(M(i, 2));
			}
			if (max < sum)
			{
				max = sum;
			}
		}
		return max;
	}

	void MatrixCom::doRank1(osg::Matrix& M, osg::Matrix& Q)
	{
		Q.makeIdentity();
		int col = findMaxCol(M);
		if (col < 0)
		{
			return; // Rank is 0
		}
		osg::Vec3 v1 = MatrixCom::getRow3(M, col);
		makeReflector(v1, v1);
		reflectCols(M, v1);
		osg::Vec3 v2 = MatrixCom::getRow3(M, 2);
		makeReflector(v2, v2);
		reflectRows(M, v2);
		float s = M(2,2);
		if (s < 0.0)
		{
			Q(2,2) = -1.0;
		}
		reflectCols(Q, v1);
		reflectRows(Q, v2);
	}

	void MatrixCom::doRank2(osg::Matrix& M, const osg::Matrix& MadjT, osg::Matrix& Q)
	{
		int col = findMaxCol(MadjT);
		if (col < 0)
		{
			doRank1(M, Q);
			return;
		}
		osg::Vec3 v1 = MatrixCom::getRow3(MadjT, col);
		makeReflector(v1, v1);
		reflectCols(M, v1);
		osg::Vec3 v2 = MatrixCom::getRow3(M, 0) ^ MatrixCom::getRow3(M, 1);
		makeReflector(v2, v2);
		reflectRows(M, v2);
		float w = M(0, 0);
		float x = M(0, 1);
		float y = M(1, 0);
		float z = M(1, 1);
		float c, s, d;
		if (w * z > x * y)
		{
			c = z + w;
			s = y - x;
			d = sqrt(c * c + s * s);
			c = c / d;
			s = s / d;
			Q(0, 0) = Q(1, 1) = c;
			Q(0, 1) = -(Q(1, 0) = s);
		}
		else
		{
			c = z - w;
			s = y + x;
			d = sqrt(c * c + s * s);
			c = c / d;
			s = s / d;
			Q(0, 0) = -(Q(1, 1) = c);
			Q(0, 1) = Q(1, 0) = s;
		}
		Q(0, 2) = Q(2, 0) = Q(1, 2) = Q(2, 1) = 0.0;
		Q(2, 2) = 1.0;
		reflectCols(Q, v1);
		reflectRows(Q, v2);
	}

	float MatrixCom::decompose(const osg::Matrix& M, osg::Matrix& Q, osg::Matrix& S, osg::Vec3& T)
	{
		const float TOL = 1.0e-6f;
		// return and remove translation
		T = MatrixCom::getRow3(M, 3);
		osg::Matrix noTransM = M;
		osg::Vec4 newCol = osg::Vec4(0.0f, 0.0f, 0.0f, 1.0f);
		MatrixCom::setRow(noTransM, newCol, 3);
		osg::Matrix Mk;
		MatrixCom::transpose(Mk, noTransM);
		float M_one = normOne(Mk);
		float M_inf = normInf(Mk);
		osg::Matrix MadjTk, Ek;
		float det, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
		do
		{
			adjointTranspose(Mk, MadjTk);
			det = MatrixCom::getRow3(Mk, 0) * MatrixCom::getRow3(MadjTk, 0);
			if (det == 0.0)
			{
				doRank2(Mk, MadjTk, Mk);
				break;
			}
			MadjT_one = normOne(MadjTk);
			MadjT_inf = normInf(MadjTk);
			gamma = sqrt(sqrt((MadjT_one * MadjT_inf) / (M_one * M_inf)) / std::abs(det));
			g1 = gamma * 0.5;
			g2 = 0.5 / (gamma * det);
			Ek = Mk;
			matBinOpEqualPlus(Mk, g1, Mk, g2, MadjTk);
			matCopyMinusEqual(Ek, Mk);
			E_one = normOne(Ek);
			M_one = normOne(Mk);
			M_inf = normInf(Mk);

		} while (E_one > (M_one * TOL));
		MatrixCom::transpose(Q, Mk);
		S = Mk * noTransM;
		for (int i = 0; i < 3; ++i)
		{
			for (int j = i; j < 3; ++j)
			{
				S(i, j) = S(j, i) = 0.5 * (S(i, j) + S(j, i));
			}
		}
		return det;
	}
}